/****
 * @file: armor_recog.hpp
 * @auther: artinx algorithm group 2020
 * @brief: class for armor detection
 TODO: 
 * decision making: choosing the best armor in @armor_detected and save it to @to_aim
 * testbench and verification: Armor_recog::detect_rune()
 * parameter tuning: Armor_recog::detect_armor() && Armor_recog::detect_rune()
 * sentry_mode not implemented yet
 * */

#ifndef ARMOR_RECOG_H
#define ARMOR_RECOG_H

#include <opencv2/highgui.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>
#include <iostream>
#include <chrono>

#define RED 0
#define BLUE 1
#define THRESHOLD_MAX_COLOR_RED 70
#define THRESHOLD_MAX_COLOR_BLUE 80
#define BRIGHTNESS_THRES_RED 70
#define BRIGHTNESS_THRES_BLUE 80
#define DELTA_ANGLE_THRESHOLD 11
#define ARMOR_LENGTH_WIDTH_RATIO_SMALL 2.26f
#define BAR_REAL_LENGTH 55.31f // the real length of light bar, unit: mm
#define ARMOR_LENGTH_WIDTH_RATIO_LARGE 4.18f

#define square(a) ((a) * (a))

using namespace cv;
using namespace std;

struct Armor{
    /*
        ***IMPORTANT***
        call @method isEmpty() to see if the struct is empty or not
        if distance == -1 ---> empty
        else: not 100 percently not empty XD
    */
    cv::RotatedRect bound;  
    cv::Point2f center;
    float distance;  //distance of two RGB bar in the image unit::pixel
    float barLen; // average length of light bar
    Point3d position;  //?not used yet
    Point3d* positions;  //?not used yet
    Point3d velocity;   //not implemented yet
    Point2f v_img;  //the speed x:vx, y:vy
    std::chrono::time_point<std::chrono::steady_clock> now;
    float confidence;  //the confident of the existence of the Armor
    Armor(RotatedRect bound, float distance) {
        this->bound;
        this->distance = distance;
        positions = 0;
    }
    Armor() {
        distance = -1;
    }
    bool isEmpty() {
        return distance == -1;
    }
};


class Armor_recog {
    public:
        const int RUNE_BLOCK_DISTANCE_THRES = 60;
        const int k_FRAME_CNT_THRESHOLD = 10;
        const float ARMOR_HEIGHT_BAR_LENGTH_RATIO = 4.f;

        std::vector<Armor> armor_detected;  //all the Armor that's been detected after call function: armor_detect()
        Armor to_aim;


        /// @brief constructor for Armor_recog
        /// @param enemyColor: the color of the oppsite side, RED stands for RED, BLUE stands for BLUE
        /// @param sentry_mode: 0 for not for sentry, 1 for yes
        Armor_recog(int enemyColor, int sentry_mode);

        /// @brief For rune detect. Detect the panel to hit and save it to armor_detected
        ///         return false if no Armor is detected
        /// @param src: the grabbed image frame
        bool detect_rune(Mat &src);
        
        /// @brief draw lines and rrect in @current_frame, and display it, for test purpose
        void showImage();
        
        // /// @brief 
        // void showVideo(String path);
       
        /// @brief For armor detect. Detect the armors in @param src and save it to armor_detected
        /// @param src: the input image
        void detect_armor(Mat &src);  //process image into armors
        
        private:
        int threshold_color;
        int threshold_brightness;
        int enemyColor;  //BLUE stands for enemy color is blue, RED stands fo enemy color is red
        int sentry_mode; //not defined yet
        std::chrono::time_point<std::chrono::steady_clock> now;
        cv::KalmanFilter KF;
        cv::Mat measurement;
        cv::Mat current_frame;
        cv::Mat processed_frame;  
        std::vector<cv::RotatedRect> edges_detected;
        

        Armor edge_to_armor(cv::RotatedRect &a, cv::RotatedRect &b, double width);

        /// @brief judge all the armors obtained in one detection based on previous data
        /// TODO: implement this
        bool judgeArmors();
        
        /// @brief preprocessing for rune detection
        void pre_processing_rune();


        void KF_initialize();
        
        /// @brief read frame in, return false if the frame is empty
        /// @param frame  
        bool read_frame(cv::Mat frame);
        
        
        /// @brief  preprocessing for normal armor detection
        void pre_process(); 

       
        /// @brief  detects possible edges after preprocess, 
        ///         invalid edges like nearly horizontal ones and those with invalid length/width ratio
        void edge_detect(); 
        
        
        /// @brief transfer edged_detected into ${Armor} struct
        bool armor_detect();
    

        /// @brief clear armors and edges detected before
        void clean();  

        /// @brief get the optimal target armor from @variable armor_detected
        ///         for sentry, avoid specific car
        ///         for others, get the armor with the biggest area
        /// TODO: implement this or send all to EMBEDED
        /// @return an Armor struct
        bool setAim();
        
        float con_disjunction_ratio(const Armor &former, const Armor &latter);  //return disjunction/conjunction ratio of two armor
};

#endif